package controls;

import gameAppLayer.enums.Constants;

import com.jme3.math.FastMath;
import com.jme3.math.Quaternion;
import com.jme3.math.Vector3f;
import com.jme3.renderer.RenderManager;
import com.jme3.renderer.ViewPort;
import com.jme3.scene.Spatial;
import com.jme3.scene.control.AbstractControl;
import com.jme3.scene.control.Control;

/*
 * Orients spatial in given direction given acceleration rates
 */
public class OrientationControl extends AbstractControl {

	
	private float maxW; //maximum absolute angular velocity, rad/s
	
	private Quaternion orientation; //current orientation of spatial
	private float[] currAngles = new float[3]; //holds specific x,y,z angles from orientation
	private Vector3f direction; //desired direction of spatial
	
	private float[] omegas = new float[3]; //current angular velocity, rad/s	
	private float[] alphas = new float[3]; //current angular acceleration, rad/s/s
	
	//private float[] maxOmegas = new float[3];
	private float[] maxAlphas = new float[3]; //max allowable acceleration rad/s/s
	
	private float[] turnoverAngles = new float[3]; //angles at which acceleration changes signs from pos to neg.
	
	private float tol = (float) (1*Math.PI/180); //error tolerance in radians (1 deg)
	private float wTol = 0.01f; //velocity error tolerance
	
	//private int index = 0;
	
	public enum States {
		
		POSACCEL, //positive theta
		NEGACCEL, //negative theta
		STOPPING, //command to halt all rotation in given direction
		STOPPED;
		
	}
	
	private States[] currState = new States[3];
	
	public OrientationControl(){}
	
	public OrientationControl(Spatial spatial) {
		this.setSpatial(spatial);
		this.orientation = spatial.getLocalRotation();
		currAngles[0] = orientation.getX();
		currAngles[1] = orientation.getY();
		currAngles[2] = orientation.getZ();
	}
	
	//contains logic to keep state machine flowing correctly	
	public void setState(States nextState) {
		
	}
	
	/*
	 * Determines angle at which acceleration changes sign given angle between
	 * current rotation and final rotation.  Uses index(i) for omega and alpha values
	 */
	private float calcAccelTurnover(float fullAngle, int i) {
		
		float a = alphas[i];
		float b = 2*omegas[i];
		float c = omegas[i]*omegas[i]*0.5f/alphas[i] - fullAngle;
		
		float t1 = (-b + FastMath.sqrt(b*b - 4*a*c))/(2*a); //time to max velocity, acceleration changeover
		float thetaTurnover = omegas[i]*t1+0.5f*alphas[i]*t1*t1;
		
		return thetaTurnover;		
	}
	
	/*
	 * Rotates spatial to given vector's direction
	 */
	public void rotateToHeading(Vector3f heading) {
		heading.normalize();		
		this.direction = heading;
		
		//boolean[] directions = new boolean[3]; //true = +theta, false = -theta
		
		for (int i = 0; i < 3; i++) {		
			//determine direction to rotate by determining if new vector is in + or - direction			
			Vector3f currRotation = orientation.getRotationColumn(i);
			
			//angle to traverse between current rotation and final heading
			float angleDiff = direction.angleBetween(currRotation); 
	//QQQNOTE:  ANGLES ARE ALWAYS POSITIVE!!!  Check for and correct this!
			//Check if currently stopped
			if (currState[i].equals(States.STOPPED)) {
				if (angleDiff > Math.PI) {
					//if new heading to "right", accelerate in that direction
					currState[i] = States.NEGACCEL;
				}
				else if ((angleDiff <= Math.PI) && (angleDiff >0)) {
					//if new heading to "left", accelerate in that direction
					currState[i] = States.POSACCEL; 
				}				
				else { //new heading same as old
					currState[i] = States.STOPPED;
				}
			}
			else { //currently accelerating in some direction
				//calculate final orientation if deceleration started now and rotation was eventually halted
				float timetoStop = omegas[i]/alphas[i];
				float stoppingAngle = currAngles[i] + omegas[i]*timetoStop + 0.5f*alphas[i]*timetoStop*timetoStop;
				
				//Difference between angle if object decelerated immediately (and stopped) and angle 
				//object needs to traverse to align to new heading
				float angleCheck = stoppingAngle - angleDiff;				
				
				//determine whether new heading is left (+) or right (-) from estimated complete stopping point
				if (angleCheck > Math.PI) { //right (-)
					if (omegas[i] > 0) { 
						//stop all movement to the left, bring object to a halt, to eventually reverse to right (-)
						currState[i] = States.STOPPING;
					}
					else { //continue acceleration to right (-)
						currState[i] = States.NEGACCEL;
					}					
				}
				else if ((angleCheck <= Math.PI) && (angleCheck >0)) { //left (+)
					if (omegas[i] >= 0) { //continue rotating to left (+)
						currState[i] = States.POSACCEL;
					}
					else { //stop all movement to right, bring object to halt, eventually reverse to left (+)
						currState[i] = States.STOPPING;
					}					
				}	
				else { //theta = 0, object can stop exactly at correct heading
					currState[i] = States.STOPPING;
				}
			}	
			//update turnover angle estimation given new heading
			turnoverAngles[i] = calcAccelTurnover(angleDiff, i);
		}
	}
	
	@Override
	protected void controlUpdate(float tpf) {
		//update orientation
		orientation = spatial.getLocalRotation();
		//update currAngles
		currAngles[0] = orientation.getX();
		currAngles[1] = orientation.getY();
		currAngles[2] = orientation.getZ();
		
		float angleRemaining;		
		
		for (int i = 0; i < 3; i++) {
			//calculate proximity to turnover.  If at or past, switch acceleration direction
			angleRemaining = currAngles[i] - turnoverAngles[i];
			
			if (currState[i].equals(States.STOPPED)) {
				//check if we are at heading
				Vector3f currRotation = orientation.getRotationColumn(i);
				float theta = currRotation.angleBetween(direction);
				if (FastMath.abs(theta) > tol) {
					//not at heading
					//accelerate to turnover
					if (angleRemaining > tol) {
						currState[i] = States.NEGACCEL;
						alphas[i] = maxAlphas[i]*-1;
					}
					else if (angleRemaining < tol) {
						currState[i] = States.POSACCEL;
						alphas[i] = maxAlphas[i];
					}					
				}
				else { //at heading, do nothing (or snap to heading?)
					
				}				
			}
			else if (currState[i].equals(States.STOPPING)) {
				//stop rotation if velocity is small enough
				if (FastMath.abs(omegas[i]) < wTol) {
					currState[i] = States.STOPPED;
					omegas[i] = 0.0f;
					alphas[i] = 0.0f;					
				}
			}
			//what happens if we miss the tol window?  never switches over
			else { //if (currState[i].equals(States.POSACCEL)) {
				if ((FastMath.abs(angleRemaining) <tol)) {
					currState[i] = States.STOPPING;
					alphas[i]*= -1;
				}				
			}
//			else if (currState[i].equals(States.NEGACCEL)) {
//				if ((FastMath.abs(angleRemaining) < tol)) {
//					currState[i] = States.STOPPING;
//					alphas[i]*= -1;
//				}				
//			}
					
			//update rotation by applying acceleration, velocity
			currAngles[i] = omegas[i]*tpf + 0.5f*alphas[i]*tpf*tpf;
			omegas[i] += alphas[i]*tpf; //velocity at next timestep
			
			//update orientation
			orientation.fromAngles(currAngles);
			spatial.setLocalRotation(orientation);
			
			//update turnover angles
			
		}
	}
	
	
	
	
	
		
		
//		if (currState.equals(States.ACCELERATING)) {
//			currState = States.DECELERATING;
//		}
//		
//		Quaternion base = new Quaternion(spatial.getLocalRotation());
//		Quaternion delta = new Quaternion(base);
//		delta.lookAt(heading, Constants.upDir);
//		
//		/*
//		 * q now contains quaternion representing full rotation of spatial.  Spatial has NOT been rotated yet
//		 * Next, we have to properly conduct the rotation
//		 */
//		
//		//get angles of rotation
//		delta.subtract(base);
//		
//		/*
//		 * The rotation can be broken into 2 halves--acceleration and deceleration, switching from one to the
//		 * other at the halfway point of each angle change
//		 */
//		//determine time necessary to accomplish each half of rotation
//		float[] deltas = {delta.getX()/2.0f, delta.getY()/2.0f, delta.getZ()/2.0f};
//		float[] times = new float[3];
//		float longestTime = 0;
//		//find length of time for each rotation, given current rotation velocities and max accelerations
//		for (int i = 0; i < deltas.length; i++) {
//			times[i] = (-omegas[i] + FastMath.sqrt(omegas[i]*omegas[i] + 2*alphas[i]*deltas[i]))/alphas[i];
//			if (times[i] > longestTime) {
//				longestTime = times[i];
//			}
//		}		
//	}
	
//	/*
//	 * Rotates spatial in direction of given vector, up to maxVel
//	 */
//	public void rotateInDirection(Vector3f direction, float maxW) {
//		this.direction = direction;
//	}
	
//	/*
//	 * Rotates spatial in direction of given vector, no cap on velocity
//	 */
//	public void rotateInDirection(Vector3f direction) {
//		
//	}
	
//	public void addRotationAxis(Vector3f axis, float angVel, float angAccel) {
//		if (index > 2) {
//			return;
//		}
//		
//		rotations[index] = axis;
//		omegas[index] = angVel;
//		alphas[index] = angAccel;
//		index++;
//	}
	
	public void setDirection(Vector3f direction) {
		this.direction = direction;
	}
	
	/*
	 * Set starting angular velocities, plus max angular accelerations
	 */
	public void setXYZRotations(
			float wxInit, float axMax,
			float wyInit, float ayMax,
			float wzInit, float azMax) {
		
		rotations[0] = new Vector3f(1,0,0);
		omegas[0] = new Vector3f(wxInit, 0,0);
		alphas[0] = new Vector3f(axMax, 0,0);
		
		rotations[1] = new Vector3f(0,1,0);
		omegas[1] = new Vector3f(0,wyInit,0);
		alphas[1] = new Vector3f(0,ayMax,0);
		
		rotations[2] = new Vector3f(0,0,1);
		omegas[2] = new Vector3f(0,0, wzInit);
		alphas[2] = new Vector3f(0,0, azMax);
	}
	
//	public void setState(States newState) {
//		currState = newState;
//	}
	
	public Control cloneForSpatial(Spatial spatial) {
		OrientationControl other = new OrientationControl(spatial);
		return other;
	}

//	@Override
//	protected void controlUpdate(float tpf) {
//		
//		Vector3f vAdd; //velocity adder, derived as accel*tpf
//		Vector3f rAdd; //rotation adder, derived as vel*tpf
//		
//		switch (currState) {
//		case ACCELERATING: 
//			for (int i = 0; i < rotations.length; i++) {
//				vAdd = alphas[i].mult(tpf);
//				omegas[i].addLocal(vAdd); //update velocity by adding acceleration for this slice of time
//				
//				rAdd = omegas[i].mult(tpf);
//				rotations[i].addLocal(rAdd);  //update rotation by adding velocity for this time slice
//			}
//			break;
//		case DECELERATING:
//			for (int i = 0; i < rotations.length; i++) {
//				vAdd = alphas[i].mult(tpf);
//				omegas[i].subtractLocal(vAdd); //update velocity by subtracting acceleration for this slice of time
//				
//				rAdd = omegas[i].mult(tpf);
//				rotations[i].addLocal(rAdd);  //update rotation by adding velocity for this time slice
//			}			
//			break;
//		case ROTATING:
//			//do nothing			
//			break;
//		case STOPPED:
//			//do nothing
//			break;
//		default:
//			break;		
//		}
//		
//	}

	@Override
	protected void controlRender(RenderManager rm, ViewPort vp) {
		// TODO Auto-generated method stub
		
	}

}
